#ifndef _PATH_H_
#define _PATH_H_

#include <iostream>
using std::cout;
using std::endl;

#include "point.h" 

typedef struct p {
	Point p;
	struct p *next;
} Path;

Path* addToEndOfPath(Path *path, Point p) {
	Path *newP, *tmp=path;

	newP = new Path;
	newP->p = p;
	newP->next = NULL;

	if(tmp == NULL) {
	        return newP;
	} else {
	        while(tmp->next) {
	                tmp = tmp->next;
	        }
	        tmp->next = newP;
	}
	return path;
}

Path* addToStartOfPath(Path *path, Point p) {
	Path *newP;

	newP = new Path;
	newP->p = p;
	newP->next = path;
	path = newP;
	return path;
}

Path* removeFromPath(Path *path, Point p) {
	Path *prevP;
	Path *currP;
	Path *tmp;

	if(path == NULL) {
		return path;
	}

	if(path->p.x == p.x && path->p.y == p.y) {
		tmp = path;
		path = path->next;
		delete tmp;
		return path;
	} else {
		prevP = path;
		currP = path->next;

		while(currP != NULL && (currP->p.x != p.x || currP->p.y != p.y)) {
			prevP = currP;
			currP = currP->next;
		}

		if(currP != NULL) {
			tmp = currP;
			prevP->next = currP->next;
			delete tmp;
			return path;
		}
	}
	return path;
}

bool inPath(Path *path, Point p) {
	if(path != NULL) {
		if(path->p.x == p.x && path->p.y == p.y) {
			return true;
		} else {
			return inPath(path->next, p);
		}
	} else {
		return false;
	}
}

Point findParent(Path *path, Point p) {
	if(path != NULL) {
		if(p.px == path->p.x && p.py == path->p.y) {
			Point ret(path->p.x, path->p.y, path->p.px, path->p.py, path->p.pgScore);
			ret.gScore = path->p.gScore;
			ret.fScore = path->p.fScore;
			return ret;
		} else {
			return findParent(path->next, p);
		}
	} else {
		return p;
	}
}

Point findPoint(Path *path, int x, int y) {
	if(path != NULL) {
		if(x == path->p.x && y == path->p.y) {
			Point ret(x, y, path->p.px, path->p.py, path->p.pgScore);
			ret.gScore = path->p.gScore;
			ret.fScore = path->p.fScore;
			return ret;
		} else {
			return findPoint(path->next, x, y);
		}
	} else {
		return Point(x, y);
	}
}

void printPath(Path *path, bool printParent) {
	if(path != NULL) {
		cout << "(" << path->p.x << ", " << path->p.y; 
		if(printParent) {
			cout << " " << "[" << path->p.px << ", " << path->p.py << "]";
		}
		cout << ")...";
		printPath(path->next, printParent);
	} else {
		cout << "END" << endl;
	}
}

#endif
